﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_CreateHRM.cs
 * \brief	This file contains the function that creates the HighResolutionMap.
 *  
 * This file contains the implementation of the CreateHRM function. This function
 * creates a High Resolution Map.
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.07
 * \date      2010-02-04
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-07-01 \n
 *      First implementation of the CreateHRM function.
 *      
 * - Version 0.02:   Antonio MATTA                          2009-07-09 \n
 *      Minor code added.
 *      
 * - Version 0.03:   Antonio MATTA                          2009-07-16 \n
 *      CheckHRMlimits() method added.
 *      
 * - Version 0.04:   Antonio MATTA                          2009-07-20 \n
 *      Method finished.
 *      
 * - Version 0.05:   Antonio MATTA                          2009-09-03 \n
 *      Minor documentation added.
 *      
 * - Version 0.06:   Antonio MATTA                          2009-09-28 \n
 *      Minor bugs fixed.
 *      
 * - Version 0.07: Thomas KOPFSTEDT                         2010-02-04 \n
 *   changes of class names due to cleanup in "interfaces.cs", and the 
 *   other interface classed to double names for the same class.
 *   
 * - Version 0.08:   Antonio MATTA                          2010-02-16 \n
 *      indices of the HighResMap modified in order to create properly.
 *      
 * - Version 0.09:   Antonio MATTA                          2010-02-18 \n
 *      HighResMap center values properly updated.
 *      
 * - Version 0.10:  Luis VALDES                             2010-04-27 \n
 *      Variables renamed for better clarity
 *      Other Minor changes
 */
/* ######################################################################### */

using System;
using System.Collections.Generic;
using System.Text;
using System.Globalization;
using System.IO;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_CreateHRM
     *
     *  \brief  This is the class of the High Resolution Map creation functions.
     *
     * This class grabs the incoming data and returns a High Resolution Map of the
     * robot's sorrounding.
     * 
     */
    /* ------------------------------------------------------------------------- */
    static class M44_CreateHRM
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static cl_Map2D CreateHRM( cl_RobotGPSINSState RobotEstimate,
         *                                             cl_Map2D LowResMapGlobal,
         *                                             cl_RobotData RobotData,
         *                                             cl_EosSet Sensors,
         *                                             cl_Map2D HighResMap
         *                                           )
         * 
         *  \brief From the information coming in the input classes, it generates a High
         *  Resolution Map.
         *  
         *  \param[in] cl_RobotGPSINSState RobotEstimate
         *  \param[in] cl_Map2D LowResMapGlobal
         *  \param[in] cl_RobotData RobotData
         *  \param[in] cl_EosSet Sensors
         *  \param[in] cl_Map2D HighResMap
         *
         */
        /* ------------------------------------------------------------------------- */
        public static cl_Map2D CreateHRM(  cl_Map2D HighResMap,
                                           cl_RobotGPSINSState RobotEstimate,
                                           cl_Map2D LowResMap,
                                           cl_EosSet Sensors,
                                           cl_RobotData RobotData )
        {
            #region VARIABLES
            cl_Map2D    HighResMap_old = new cl_Map2D();
            ushort       us_Scale_local = 0;
            ushort       us_Nrows = 0;
            ushort       us_Ncols = 0;
            //HighResMap.us_Nrows = (ushort)s_Nm1_local;
            //HighResMap.us_Ncols = (ushort)s_Nm2_local;
            short       s_n = 0;
            short       s_IInit = 0;
            short       s_IFinal = 0;
            short       s_JInit = 0;
            short       s_JFinal = 0;
            double      RobotDimension = 0.0f;
            double[]    i_newdouble = new double[2];
            int[]       i_Cell   = new int[2];
            int[]       i_CellPos = new int[2];
            int[]       i_IJRobot = new int[2];
            float[]     i_dp = new float[2];
            float[]     i_CenterLocal = new float[2];
            float[]     i_SupCorner = new float[2];
            float[]     i_InfCorner = new float[2];
            float[]     i_new = new float[2];
            int[]       i_old = new int[2];
           
            #endregion

            //We check if HighResMap is already declared
            if (HighResMap == null)
            {
                //If it is not declared yet, we create a new instance
                HighResMap = new cl_Map2D();
                //The scale of the HighResMap is adapted to the maximum dimension of the robot
                RobotDimension = Math.Max((RobotData.Dimensions.d_DimXneg + RobotData.Dimensions.d_DimXpos),
                                         (RobotData.Dimensions.d_DimYneg + RobotData.Dimensions.d_DimYpos));

                if (RobotDimension < 0.5)
                {
                    us_Scale_local = (ushort)5;
                }
                else if (RobotDimension > 1.0)
                {
                    us_Scale_local = M44_SensorFusionAndMappingService.i_MapRes;
                }
                else
                {
                    us_Scale_local = (ushort)15;
                }

                /*Test*/
                us_Scale_local = (ushort)5;


                s_n = (short)((M44_SensorFusionAndMappingService.i_MapRes*LowResMap.us_Scale / us_Scale_local));
                us_Nrows = (ushort)(M44_SensorFusionAndMappingService.i_MapRes*Math.Abs((short)Math.Round(2 * Sensors.f_Range / us_Scale_local)));
                us_Ncols = (ushort)(M44_SensorFusionAndMappingService.i_MapRes * Math.Abs((short)Math.Round(2 * Sensors.f_Range / us_Scale_local)));

                HighResMap.us_Nrows = us_Nrows;
                HighResMap.us_Ncols = us_Ncols;
                HighResMap.us_Scale = us_Scale_local;
                HighResMap.MapCell = new cl_MapCell[HighResMap.us_Nrows, HighResMap.us_Ncols];

                /*The HighResMap Cell structure must be created with zeros*/
                for (int i = 0; i < HighResMap.us_Nrows; i++)
                {
                    for (int j = 0; j < HighResMap.us_Ncols; j++)
                    {
                        /*Order of parameters:
                         c8_CommQuality_local = 0;
                         c8_Confidence_local = 0;
                         c8_GPSQuality_local = 0;
                         f_Height_local = 0f;
                         c8_Occupation_local = 0;
                         f_Gradient_x_local = 0f;
                         f_Gradient_y_local = 0f;
                         c8_TractionEstimation_local = 0;
                        */
                        HighResMap.MapCell[i, j] = new cl_MapCell(0, 0, 0, 0.5f, 0, 0f, 0f, 0);
                    }
                }

                i_Cell = M44_BasicClass.Coordinates2Cell(LowResMap, RobotEstimate.RobotEstimate.td_Pos);
                i_Cell = M44_BasicClass.CheckHRMlimits(LowResMap, i_Cell, us_Nrows, us_Ncols, s_n);
                i_dp = M44_BasicClass.Cell2Coordinates(LowResMap, i_Cell);
                
                i_CenterLocal[0] = i_dp[0];
                i_CenterLocal[1] = i_dp[1];

                HighResMap.Center.d_x = i_CenterLocal[0];
                HighResMap.Center.d_y = i_CenterLocal[1];

                /*We take the information of the global map and create the local map*/
                s_IInit = (short)(i_Cell[0] -  (us_Nrows / 2 * 1 / s_n) + 1 );
                s_IFinal = (short)(i_Cell[0] + (us_Nrows / 2 * 1 / s_n) );
                s_JInit = (short)(i_Cell[1] -  (us_Ncols / 2 * 1 / s_n) );
                s_JFinal = (short)(i_Cell[1] + (us_Ncols / 2 * 1 / s_n) - 1 );
          
                /*Fill the HighResMap with data coming from the LowResMap*/
                for (int I = s_IInit; I <= s_IFinal; I++)
                {
                    for (int J = s_JInit; J <= s_JFinal; J++)
                    {
                        for (int i = ((I - s_IInit) * s_n); i < ((I - s_IInit) * s_n + s_n); i++)
                        {
                            for (int j = ((J - s_JInit) * s_n); j < (J - s_JInit) * s_n + s_n; j++)
                            {
                                HighResMap.MapCell[i, j].c8_CommQuality = LowResMap.MapCell[I, J].c8_CommQuality;
                                HighResMap.MapCell[i, j].c8_Confidence = LowResMap.MapCell[I, J].c8_Confidence;
                                HighResMap.MapCell[i, j].c8_GPSQuality = LowResMap.MapCell[I, J].c8_GPSQuality;
                                HighResMap.MapCell[i, j].f_Height = LowResMap.MapCell[I, J].f_Height;
                                HighResMap.MapCell[i, j].c8_Occupation = LowResMap.MapCell[I, J].c8_Occupation;
                                HighResMap.MapCell[i, j].f_Gradient_x = LowResMap.MapCell[I, J].f_Gradient_x;
                                HighResMap.MapCell[i, j].f_Gradient_y = LowResMap.MapCell[I, J].f_Gradient_y;
                                HighResMap.MapCell[i, j].c8_TractionEstimation = LowResMap.MapCell[I, J].c8_TractionEstimation;
                            }
                        }
                    }
                }
            }
            else
            {
                /* A HighResMap already exists. We check if it's necessary to move it.
                 * We check if the laser beam fits into the local map. If it does not
                 * we move the local map.
                */

                HighResMap_old = HighResMap;
                i_IJRobot = M44_BasicClass.Coordinates2Cell_High(HighResMap, RobotEstimate.RobotEstimate.td_Pos);

                if ( ( Math.Abs(i_IJRobot[0]-1) < Sensors.f_Range/((float)HighResMap.us_Scale/M44_SensorFusionAndMappingService.i_MapRes) + 2 ) ||
                     ( Math.Abs(HighResMap.us_Nrows -i_IJRobot[0]) < Sensors.f_Range/((float)HighResMap.us_Scale/M44_SensorFusionAndMappingService.i_MapRes) + 2 ) ||
                     (Math.Abs(i_IJRobot[1] - 1) < Sensors.f_Range / ((float)HighResMap.us_Scale / M44_SensorFusionAndMappingService.i_MapRes) + 2) ||
                     (Math.Abs(HighResMap.us_Ncols - i_IJRobot[1]) < Sensors.f_Range / ((float)HighResMap.us_Scale / M44_SensorFusionAndMappingService.i_MapRes) + 2)
                    )
                {
                    i_CellPos[0] = 0;// 1;
                    i_CellPos[1] = 0;// 1;
                    i_SupCorner = M44_BasicClass.Cell2Coordinates_High(HighResMap_old, i_CellPos);
                    i_CellPos[0] = (int)HighResMap_old.us_Nrows;
                    i_CellPos[1] = (int)HighResMap_old.us_Ncols;
                    i_InfCorner = M44_BasicClass.Cell2Coordinates_High(HighResMap_old, i_CellPos);

                    us_Scale_local = HighResMap.us_Scale;
                    us_Nrows = HighResMap.us_Nrows;
                    us_Ncols = HighResMap.us_Ncols;
                    s_n = (short)(M44_SensorFusionAndMappingService.i_MapRes*LowResMap.us_Scale / HighResMap.us_Scale);

                    /*We move the map to the next position wich is the actual robot position*/
                    i_Cell = M44_BasicClass.Coordinates2Cell(LowResMap, RobotEstimate.RobotEstimate.td_Pos);
                    i_Cell = M44_BasicClass.CheckHRMlimits(LowResMap, i_Cell, us_Nrows, us_Ncols, s_n);
                    i_dp = M44_BasicClass.Cell2Coordinates(LowResMap, i_Cell);

                    i_CenterLocal[0] = i_dp[0];
                    i_CenterLocal[1] = i_dp[1];

                    HighResMap.Center.d_x = i_CenterLocal[0];
                    HighResMap.Center.d_y = i_CenterLocal[1];

                    /*We take the information of the global map and create the local map*/
                    s_IInit = (short)(i_Cell[0] - (us_Nrows) / 2 * 1 / s_n + 1);
                    s_IFinal = (short)(i_Cell[0] + (us_Nrows) / 2 * 1 / s_n);
                    s_JInit = (short)(i_Cell[1] - (us_Ncols) / 2 * 1 / s_n);
                    s_JFinal = (short)(i_Cell[1] + (us_Ncols) / 2 * 1 / s_n - 1);

                    /*Fill the HighResMap with data coming from the LowResMapGlobal*/
                    for (int I = s_IInit; I <= s_IFinal; I++)
                    {
                        for (int J = s_JInit; J <= s_JFinal; J++)
                        {
                            for (int i = ((I - s_IInit) * s_n); i < ((I - s_IInit) * s_n + s_n); i++)
                            {
                                for (int j = ((J - s_JInit) * s_n); j < (J - s_JInit) * s_n + s_n; j++)
                                {
                                    HighResMap.MapCell[i, j].c8_CommQuality = LowResMap.MapCell[I, J].c8_CommQuality;
                                    HighResMap.MapCell[i, j].c8_Confidence = LowResMap.MapCell[I, J].c8_Confidence;
                                    HighResMap.MapCell[i, j].c8_GPSQuality = LowResMap.MapCell[I, J].c8_GPSQuality;
                                    HighResMap.MapCell[i, j].f_Height = LowResMap.MapCell[I, J].f_Height;
                                    HighResMap.MapCell[i, j].c8_Occupation = LowResMap.MapCell[I, J].c8_Occupation;
                                    HighResMap.MapCell[i, j].f_Gradient_x = LowResMap.MapCell[I, J].f_Gradient_x;
                                    HighResMap.MapCell[i, j].f_Gradient_y = LowResMap.MapCell[I, J].f_Gradient_y;
                                    HighResMap.MapCell[i, j].c8_TractionEstimation = LowResMap.MapCell[I, J].c8_TractionEstimation;
                                }
                            }
                        }
                    }

                    for (int i = 0; i < us_Nrows; i++)
                    {
                        for (int j = 0; j < us_Ncols; j++)
                        {
                            i_CellPos[0] = i;
                            i_CellPos[1] = j;
                            i_new = M44_BasicClass.Cell2Coordinates_High(HighResMap, i_CellPos);
                            if( (i_new[0]>i_SupCorner[0]) && ( i_new[0]< i_InfCorner[0]) && (i_new[1]>i_SupCorner[1]) && (i_new[1]< i_InfCorner[1]) )
                            {
                                i_newdouble[0] = (double)i_new[0];
                                i_newdouble[1] = (double)i_new[1];
                                i_old = M44_BasicClass.Coordinates2Cell_High(HighResMap_old, i_newdouble);
                                HighResMap.MapCell[i, j].c8_CommQuality     = HighResMap_old.MapCell[i_old[0], i_old[1]].c8_CommQuality;
                                HighResMap.MapCell[i, j].c8_Confidence      = HighResMap_old.MapCell[i_old[0], i_old[1]].c8_Confidence;
                                HighResMap.MapCell[i, j].c8_GPSQuality      = HighResMap_old.MapCell[i_old[0], i_old[1]].c8_GPSQuality;
                                HighResMap.MapCell[i, j].f_Height           = HighResMap_old.MapCell[i_old[0], i_old[1]].f_Height;
                                HighResMap.MapCell[i, j].c8_Occupation      = HighResMap_old.MapCell[i_old[0], i_old[1]].c8_Occupation;
                                HighResMap.MapCell[i, j].f_Gradient_x       = HighResMap_old.MapCell[i_old[0], i_old[1]].f_Gradient_x;
                                HighResMap.MapCell[i, j].f_Gradient_y       = HighResMap_old.MapCell[i_old[0], i_old[1]].f_Gradient_y;
                                HighResMap.MapCell[i, j].c8_TractionEstimation = HighResMap_old.MapCell[i_old[0], i_old[1]].c8_TractionEstimation;
                            }
                        }
                    }
                }
            }
            return HighResMap;
        }
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */
